/**
 ******************************************************************************
 * @file    PID.h
 * @author  Li Zhenxu
 * @version V1.0.0
 * @date    2022/12/22
 * @brief
 ******************************************************************************
 * @attention
 *
 ******************************************************************************
 */
 
#ifndef _PID_H_
#define _PID_H_
#include <stdint.h>
#include "bsp_dwt.h"

enum pid_mode
{
    pid_position = 0,
    pid_delta
};

typedef struct
{
	uint8_t mode;
	
	float Kp;
	float Ki;
	float Kd;
	
	float Ref;
	float Mea;
	float Last_Mea;
	
	float Err[3]; 
	float Dbuf[3];
	
	float Pout;
	float Iout;
	float Dout;
	float Iterm;
	
	float Out;
	float MaxOut;
	float MinOut;
	
	float feed_output;
	
} PID_t;

void pid_init(PID_t *PID , float Kp , float Ki , float Kd , uint8_t mode);
float pid_calculate(PID_t *pid , float measure , float ref , float dt);
float pid_cascade_calculate(PID_t *PID_Angle , PID_t *PID_Velocity ,  float angle , float velocity , float target_angle , float dt);
float pid_cascade_calculate_feedforward(PID_t *PID_Angle , PID_t *PID_Velocity ,  float angle , float velocity , float target_angle , float dt , float feed);

#endif
